﻿#include "pid.h"

Pid::Pid()
   : kp_(0)
   , ki_(0)
   , kd_(0)
   , integral_err_(0)
   , dif_err_(0)
   , pre_err_(0)
   , last_err_(0)
   , integral_saturation_(9999999)
   , adjust_limit_(9999999)
{
}


Pid::Pid(double kp, double ki, double kd, double IntegralSaturation, double adjustLimit)
   : kp_(kp)
   , ki_(ki)
   , kd_(kd)
   , integral_err_(0)
   , dif_err_(0)
   , pre_err_(0)
   , last_err_(0)
   , integral_saturation_(IntegralSaturation)
   , adjust_limit_(adjustLimit)
{
}


double Pid::positionPid(double err)
{
   integral_err_ += err;             //积分项
   dif_err_       = err - last_err_; //微分项
   last_err_      = err;

   double integ_item;
   integ_item = (ki_ * integral_err_) > integral_saturation_ ? integral_saturation_ : (ki_ * integral_err_);
   integ_item = (ki_ * integral_err_) < (-integral_saturation_) ? (-integral_saturation_) : (ki_ * integral_err_);

//   qDebug() << "kp:" << kp_ * err << " ki:" << ki_ * integral_err_ << " kd:" << kd_ * dif_err_;

   double adj = ((kp_ * err) + integ_item + (kd_ * dif_err_));

   adj = (adj > adjust_limit_) ? adjust_limit_ : adj;
   adj = (adj < -adjust_limit_) ? (-adjust_limit_) : adj;

   return adj;
}


double Pid::incrementPid(double err)
{
   double result;

   /* 输出量 = 比例常数 * 当前误差 - 积分常数 * 上一次误差 + 微分常数 * 前一次误差 */
   result = (kp_ * err) - (ki_ * last_err_) + (kd_ * pre_err_);
   /* 更新前一次误差变量 */
   pre_err_ = last_err_;
   /* 更新上一次误差变量 */
   last_err_ = err;

   return result;
}


void Pid::clearErr()
{
   dif_err_      = 0;
   pre_err_      = 0;
   last_err_     = 0;
   integral_err_ = 0;
}


double Pid::integralSaturation() const
{
   return integral_saturation_;
}


void Pid::setIntegralSaturation(double integral_saturation)
{
   integral_saturation_ = integral_saturation;
}


double Pid::adjustLimit() const
{
   return adjust_limit_;
}


void Pid::setAdjustLimit(double adjust_limit)
{
   adjust_limit_ = adjust_limit;
}
